#include <cmath>
#include <cfloat>

#include "Quaternion.h"
#include "Vector3.h"
#include "Matrix33.h"
#include "Matrix44.h"

using namespace gti;

quaternion::quaternion() 
{
}

quaternion::quaternion(const quaternion& q): x(q.x),y(q.y),z(q.z),w(q.w)
{ 
}

quaternion::quaternion(const float X, const float Y,  const float Z, const float W) : x(X),y(Y),z(Z),w(W)
{ 
}

quaternion quaternion::invert() const
{
	return quaternion(-x,-y,-z,w);
}

quaternion quaternion::conjugate() const
{
	return quaternion(-x,-y,-z,w);
}

void quaternion::set(const float X, const float Y,  const float Z, const float W)
{
	x = X;  y = Y; z = Z; w = W; 
}

quaternion::quaternion(const float* q)
{
	x = q[0];
	y = q[1];
	z = q[2];
	w = q[3];
}

void quaternion::identity()
{
	x=y=z=0.0f; w=1.0f;
}

void quaternion::slerp(const quaternion& b, float t)
{
	//quaternion a(*this);
	*this = Qslerp(*this,b,t);
}

void quaternion::slerp(const quaternion& q2, float t, quaternion &q3) const
{
	q3 = Qslerp(*this,q2,t);
}

void quaternion::lerp(const quaternion& b, float t)
{
	*this = Qlerp(*this,b,t);
}

void quaternion::lerp(const quaternion& q2, float t, quaternion &q3) const
{
	q3 = Qlerp(*this,q2,t);
}

void quaternion::setAxisAngle(const vector3f& axis, const float angle) 
{
		float s;
		s = sinf(angle * 0.5f);

		x = axis.x * s;
		y = axis.y * s;
		z = axis.z * s;
		w = cosf(angle * 0.5f);
}

quaternion::quaternion(const vector3f& axis, float angle) 
{
	setAxisAngle(axis,angle);
}

void quaternion::operator *= (const quaternion &q) 
{
	quaternion quaternion = *this * q;
	*this = quaternion;
}

void quaternion::operator*=(const vector3f& v)
{
	*this=*this*v;
}

void quaternion::operator += (const quaternion &q)
{
	x += q.x;
	y += q.y;
	z += q.z;
	w += q.w;
}

quaternion gti::operator * (const quaternion& q1, const quaternion& q2) 
{
	quaternion q;

	q.x = q1.y*q2.z - q1.z*q2.y + q1.w*q2.x + q1.x*q2.w;
	q.y = q1.z*q2.x - q1.x*q2.z + q1.w*q2.y + q1.y*q2.w;
	q.z = q1.x*q2.y - q1.y*q2.x + q1.w*q2.z + q1.z*q2.w;
	q.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
	return q;
}

/*
//http://www.cs.yorku.ca/~arlene/aquasim/mymath.c
quaternion operator * (const quaternion& p, const quaternion& q)
{
		return quaternion(
		     p.w*q.x + p.x*q.w + p.y*q.z - p.z*q.y,
		     p.w*q.y + p.y*q.w + p.z*q.x - p.x*q.z,
		     p.w*q.z + p.z*q.w + p.x*q.y - p.y*q.x,
				 p.w*q.w - p.x*q.x - p.y*q.y - p.z*q.z);
}
*/

quaternion gti::operator * (const quaternion &q, const vector3f& v)
{
	return	quaternion
		(
							q.w*v.x + q.y*v.z - q.z*v.y,
							q.w*v.y + q.z*v.x - q.x*v.z,
							q.w*v.z + q.x*v.y - q.y*v.x,
							-(q.x*v.x + q.y*v.y + q.z*v.z)
		);
}

void quaternion::operator *= (float f)
{
    x *= f;
    y *= f;
    z *= f;
    w *= f;
}

quaternion gti::operator * (const quaternion &q, float f)
{
    quaternion q1;
    q1.x = q.x*f;
    q1.y = q.y*f;
    q1.z = q.z*f;
    q1.w = q.w*f;
    return q1;
}

quaternion gti::operator * (float f, const quaternion &q)
{
    quaternion q1;
    q1.x = q.x*f;
    q1.y = q.y*f;
    q1.z = q.z*f;
    q1.w = q.w*f;
    return q1;
}

void quaternion::normalize() 
{
	float len = length();
	len = 1.0f/len;

	x *= len;
	y *= len;
	z *= len;
	w *= len;
}

void quaternion::computeMinimumRotation(const vector3f& rotateFrom,const vector3f& rotateTo)
{
  // Check if the vectors are valid.
  //rotateFrom.GetLength()==0.0f
  //rotateTo.GetLength()==0.0f

  vector3f from(rotateFrom);
  from.normalize();
  vector3f to(rotateTo);
  to.normalize();

  const float dot = DotProduct(from,to);
  vector3f crossvec = CrossProduct(from,to);
  const float crosslen = crossvec.length();

  if (crosslen == 0.0f)
	{
		// Parallel vectors
    // Check if they are pointing in the same direction.
    if (dot > 0.0f) 
		{
      x=y=z=0.0f;
			w=1.0f;
		}
    // Ok, so they are parallel and pointing in the opposite direction
    // of each other.
    else
		{
      // Try crossing with x axis.
      vector3f t = CrossProduct(from,vector3f(1.0f, 0.0f, 0.0f));
      // If not ok, cross with y axis.
      if(t.length() == 0.0f)
				CrossProduct(from,vector3f(0.0f, 1.0f, 0.0f));

      t.normalize();
      x=t[0];
			y=t[1];
			z=t[2];
			w=0.0f;
    }
  }
  else
	{ // Vectors are not parallel
    crossvec.normalize();
    // The fabs() wrapping is to avoid problems when `dot' "overflows"
    // a tiny wee bit, which can lead to sqrt() returning NaN.
    crossvec *= (float)sqrt(0.5f * fabs(1.0f - dot));
    // The fabs() wrapping is to avoid problems when `dot' "underflows"
    // a tiny wee bit, which can lead to sqrt() returning NaN.
    x=crossvec[0];
		y=crossvec[1];
		z=crossvec[2],
		w=(float)sqrt(0.5 * fabs(1.0 + dot));
  }
}

quaternion gti::SimpleRotation(const vector3f &a, const vector3f &b)
{
    vector3f axis = CrossProduct(a, b);
    float dot = DotProduct(a, b);
    if (dot < -1.0f + /*DOT_EPSILON*/0.001f) return quaternion(0, 1, 0, 0);
 
    quaternion result(axis.x * 0.5f, axis.y * 0.5f, axis.z * 0.5f, (dot + 1.0f) * 0.5f);
		result.normalize();
    //fast_normalize(&result);
 
    return result;
}


float quaternion::length () const
{
	return sqrtf(w * w + x * x + y * y + z * z);
}

float quaternion::squaredLength() const
{
	return w * w + x * x + y * y + z * z;
}

void quaternion::toMatrix(matrix33& matrix) const
{
	/*
	If q is guaranteed to be a unit quaternion, s will always
	be 2.  In that case, this calculation can be optimized out.
	*/
	//const float	norm = x*x + y*y + z*z + w*w;
	//const float s = (norm > 0) ? 2/norm : 0;

	const float s = 2;
	
	/*
	Precalculate coordinate products
	*/
	const float xx = x * x * s;
	const float yy = y * y * s;
	const float zz = z * z * s;
	const float xy = x * y * s;
	const float xz = x * z * s;
	const float yz = y * z * s;
	const float wx = w * x * s;
	const float wy = w * y * s;
	const float wz = w * z * s;

	/*
	Calculate 3x3 matrix from orthonormal basis
	*/

	/*
	x axis
	*/
	matrix.M[0][0] = 1.0f - (yy + zz);
	matrix.M[1][0] = xy + wz;
	matrix.M[2][0] = xz - wy;
	
	/*
	y axis
	*/
	matrix.M[0][1] = xy - wz;
	matrix.M[1][1] = 1.0f - (xx + zz);
	matrix.M[2][1] = yz + wx;
	
	/*
	z axis
	*/
	matrix.M[0][2] = xz + wy;
	matrix.M[1][2] = yz - wx;
	matrix.M[2][2] = 1.0f - (xx + yy);

}

void quaternion::toMatrix(matrix44& matrix) const
{
	/*
	If q is guaranteed to be a unit quaternion, s will always
	be 2.  In that case, this calculation can be optimized out.
	*/
	//const float	norm = x*x + y*y + z*z + w*w;
	//const float s = (norm > 0) ? 2/norm : 0;

	const float s = 2;
	
	/*
	Precalculate coordinate products
	*/
	const float xx = x * x * s;
	const float yy = y * y * s;
	const float zz = z * z * s;
	const float xy = x * y * s;
	const float xz = x * z * s;
	const float yz = y * z * s;
	const float wx = w * x * s;
	const float wy = w * y * s;
	const float wz = w * z * s;

	/*
	Calculate 3x3 matrix from orthonormal basis
	*/

	/*
	x axis
	*/
	// >>
	matrix.M[0][0] = 1.0f - (yy + zz);
	//matrix.M[0][0] = x*x-y*y-z*z+w*w;
	// <<
	matrix.M[1][0] = xy + wz;
	matrix.M[2][0] = xz - wy;
	
	/*
	y axis
	*/
	matrix.M[0][1] = xy - wz;
	// >>
	matrix.M[1][1] = 1.0f - (xx + zz);
	//matrix.M[1][1] = -x*x+y*y-z*z+w*w;
	// <<
	matrix.M[2][1] = yz + wx;
	
	/*
	z axis
	*/
	matrix.M[0][2] = xz + wy;
	matrix.M[1][2] = yz - wx;
	matrix.M[2][2] = 1.0f - (xx + yy);

	matrix._14 = matrix._24 = matrix._34 = 0;
	matrix._41 = matrix._42 = matrix._43 = 0;
	matrix._44 = 1;
}

float gti::DotProduct(const quaternion& q1, const quaternion& q2) 
{
	return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
}

bool gti::operator==(const quaternion& q1, const quaternion& q2)
{
	return ( (q1.x == q2.x) && ( q1.y == q2.y) &&
		(q1.z == q2.z) && (q1.w == q2.w) );
}

bool gti::operator!=(const quaternion& q1, const quaternion& q2)
{
	return ( (q1.x != q2.x) || ( q1.y != q2.y) ||
		(q1.z != q2.z) || (q1.w != q2.w) );
}

/*
Logarithm of a quaternion, given as:
Qlog(q) = v*a where q = [cos(a),v*sin(a)]
*/
quaternion gti::Qlog(const quaternion &q)
{
	float a = static_cast<float>(acos(q.w));
	float sina = static_cast<float>(sin(a));
	quaternion ret;
	ret.w = 0;
	if (sina > 0)
	{
		ret.x = a*q.x/sina;
		ret.y = a*q.y/sina;
		ret.z = a*q.z/sina;
	}
	else
	{
		ret.x=ret.y=ret.z=0;
	}
	return ret;
}

/*
e^quaternion given as:
Qexp(v*a) = [cos(a),vsin(a)]
*/
quaternion gti::Qexp(const quaternion &q)
{
	float a = static_cast<float>(sqrt(q.x*q.x + q.y*q.y + q.z*q.z));
	float sina = static_cast<float>(sin(a));
	float cosa = static_cast<float>(cos(a));
	quaternion ret;

	ret.w = cosa;
	if(a > 0)
	{
		ret.x = sina * q.x / a;
		ret.y = sina * q.y / a;
		ret.z = sina * q.z / a;
	}
	else
	{
		ret.x = ret.y = ret.z = 0;
	}

	return ret;
}

quaternion gti::operator + (const quaternion &q1, const quaternion& q2)
{
    return quaternion(q1.x+q2.x,q1.y+q2.y,q1.z+q2.z,q1.w+q2.w);
}

/*
Linear interpolation between two quaternions
*/
quaternion gti::Qlerp(const quaternion &q1,const quaternion &q2,float t)
{
	quaternion ret;
	//ret = q1 + t*(q2-q1);

	quaternion b;
	if (DotProduct(q1,q2)< 0.0f)
		b.set(-q2.x,-q2.y,-q2.z,-q2.w);
	else b = q2;

	ret = q1*(1-t) + b*t;

	ret.normalize();
	return ret;
}

/*
Spherical linear interpolation between two quaternions
*/
quaternion gti::Qslerp(const quaternion &q1, const quaternion &q2, float t)
{
	quaternion q3;
	float dot = DotProduct(q1,q2);

	//dot = cos(theta)
	//if (dot < 0), q1 and q2 are more than 90 degrees apart,
	//so we can invert one to reduce spinning
	
	if (dot < 0)
	{
		dot = -dot;
		q3.set(-q2.x,-q2.y,-q2.z,-q2.w);
	}
	else
	{
		q3 = q2;
	}

	if (dot < 0.95f)
	{
		float angle = static_cast<float>(acosf(dot));
		float sina,sinat,sinaomt;
		sina = sinf(angle);
		sinat = sinf(angle*t);
		sinaomt = sinf(angle*(1-t));
		return (q1*sinaomt + q3*sinat) * (1.0f/sina);
	}
	
	//if the angle is small, use linear interpolation

	else
	{
		return Qlerp(q1,q3,t);
	}

	

/*
    float fCos = DotProduct(q1,q2);

	if (fCos > 1) 
		fCos = 1;
	if (fCos < -1) 
		fCos = -1;

    float fAngle = acosf(fCos);

    if ( fabs(fAngle) < 0.0001 )
	{
		return q1;
	}

    float fSin = sinf(fAngle);
    float fInvSin = 1.0f/fSin;
    float fCoeff0 = sinf((1.0f-fT)*fAngle)*fInvSin;
    float fCoeff1 = sinf(fT*fAngle)*fInvSin;
    return fCoeff0*q1 + fCoeff1*q2;
*/
}


/*
This version of slerp, used by squad, does not check for theta > 90.
*/

quaternion gti::QslerpNoInvert(const quaternion &q1,const quaternion &q2,float t)
{
	float dot = q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;

	if (dot > -0.95f && dot < 0.95f)
	{
		float angle = static_cast<float>(acos(dot));
		float sina,sinat,sinaomt;
		sina = static_cast<float>(sin(angle));
		sinat = static_cast<float>(sin(angle*t));
		sinaomt = static_cast<float>(sin(angle*(1-t)));
		return (q1*sinaomt+q2*sinat)  * (1/sina);
	}
	/*
	if the angle is small, use linear interpolation
	*/
	else
	{
		return Qlerp(q1,q2,t);
	}
}


/*
Spherical cubic interpolation
*/
quaternion gti::Qsquad(const quaternion &q1,const quaternion &q2,const quaternion &a,const quaternion &b,float t)
{
	quaternion c,d;
	c = QslerpNoInvert(q1,q2,t);
	d = QslerpNoInvert(a,b,t);
	return QslerpNoInvert(c,d,2*t*(1-t));
}


/*
Given 3 quaternions, qn-1,qn and qn+1, calculate a control point to be used in spline interpolation
*/

quaternion& quaternion::operator -()
{
    x = -x;
    y = -y;
    z = -z;
    w = -w;

    return *this;
}

quaternion gti::Qspline(const quaternion &qnm1,const quaternion &qn,const quaternion &qnp1)
{
	quaternion qni;
	
	qni.x = -qn.x;
	qni.y = -qn.y;
	qni.z = -qn.z;
	qni.w = qn.w;

	return qn*Qexp(( -(Qlog(qni*qnm1)) + Qlog(qni*qnp1) ) * (1/4.0f));
}


void quaternion::getAxisAngle(vector3f &v, float &angle) const
{
	angle = 2.0f * acosf(w);
	float factor = 1.0f/ sqrtf(1-w*w);
	v.x = x * factor;
	v.y = y * factor;
	v.z = z * factor;
}

vector3f quaternion::rotate(const vector3f& v) const
{
	vector3f ret;
	quaternion temp(-x, -y, -z, w);
	temp *= v;
	temp *= (*this);

	ret.x = temp.x;
	ret.y = temp.y;
	ret.z = temp.z;

	return ret;
}

void quaternion::toEulerAngles(vector3f &euler) const
{
/// Local Variables ///////////////////////////////////////////////////////////
	float matrix[3][3];
	float cx,sx;
	float cy,sy;
	float cz,sz;

	const float y2 = y*y;
///////////////////////////////////////////////////////////////////////////////
	// CONVERT QUATERNION TO MATRIX - I DON'T REALLY NEED ALL OF IT
	matrix[0][0] = 1.0f - (2.0f * y2) - (2.0f * z * z);
//	matrix[0][1] = (2.0f * quat->x * quat->y) - (2.0f * quat->w * quat->z);
//	matrix[0][2] = (2.0f * quat->x * quat->z) + (2.0f * quat->w * quat->y);
	matrix[1][0] = (2.0f * x * y) + (2.0f * w * z);
//	matrix[1][1] = 1.0f - (2.0f * quat->x * quat->x) - (2.0f * quat->z * quat->z);
//	matrix[1][2] = (2.0f * quat->y * quat->z) - (2.0f * quat->w * quat->x);
	matrix[2][0] = (2.0f * x * z) - (2.0f * w * y);
	matrix[2][1] = (2.0f * y * z) + (2.0f * w * x);
	matrix[2][2] = 1.0f - (2.0f * x * x) - (2.0f * y2);

	sy = -matrix[2][0];
	cy = sqrtf(1 - (sy * sy));
	euler.y = atan2f(sy,cy);
	//euler->y = (yr * 180.0f) / (float)M_PI;

	// AVOID DIVIDE BY ZERO ERROR ONLY WHERE Y= +-90 or +-270 
	// NOT CHECKING cy BECAUSE OF PRECISION ERRORS
	if (sy != 1.0f && sy != -1.0f)	
	{
		cx = matrix[2][2] / cy;
		sx = matrix[2][1] / cy;
		//euler->x = ((float)atan2(sx,cx) * 180.0f) / (float)M_PI;	// RAD TO DEG
		euler.x = atan2f(sx,cx);

		cz = matrix[0][0] / cy;
		sz = matrix[1][0] / cy;
		//euler->z = ((float)atan2(sz,cz) * 180.0f) / (float)M_PI;	// RAD TO DEG
		euler.z = atan2f(sz,cz);
	}
	else
	{
		// SINCE Cos(Y) IS 0, I AM SCREWED.  ADOPT THE STANDARD Z = 0
		// I THINK THERE IS A WAY TO FIX THIS BUT I AM NOT SURE.  EULERS SUCK
		// NEED SOME MORE OF THE MATRIX TERMS NOW
		matrix[1][1] = 1.0f - (2.0f * x * x) - (2.0f * z * z);
		matrix[1][2] = (2.0f * y * z) - (2.0f * w * x);
		cx = matrix[1][1];
		sx = -matrix[1][2];
		//euler->x = ((float)atan2(sx,cx) * 180.0f) / (float)M_PI;	// RAD TO DEG
		euler.x = atan2f(sx,cx);

		cz = 1.0f;
		sz = 0.0f;
		//euler->z = ((float)atan2(sz,cz) * 180.0f) / (float)M_PI;	// RAD TO DEG
		euler.z = atan2f(sz,cz);
	}
}


void quaternion::setAxisAngle(float ax, float ay, float az, float angle)
{
	const float halfAngle = angle * 0.5f;
	const float s = sinf(halfAngle);

	w = cosf(halfAngle);
	x = ax * s;
	y = ay * s;
	z = az * s;
	
}

void gti::QuaternionYaw(float angle,quaternion& quat)
{
	quat.setAxisAngle(0,1,0,angle);
}

void gti::QuaternionPitch(float angle,quaternion& quat)
{
	quat.setAxisAngle(1,0,0,angle);
}

void gti::QuaternionRoll(float angle,quaternion& quat)
{
	quat.setAxisAngle(0,0,1,angle);
}

void gti::QuaternionYawPitchRoll(float yaw, float pitch, float roll,quaternion& quat)
{
// optimized version

	const float halfYaw = yaw * 0.5f;
	const float halfPitch = pitch * 0.5f;
	const float halfRoll = roll * 0.5f;

	const float c1 = cosf(halfYaw);
	const float c2 = cosf(halfRoll);
	const float c3 = cosf(halfPitch); 
	const float s1 = sinf(halfYaw);
	const float s2 = sinf(halfRoll);
	const float s3 = sinf(halfPitch);

	quat.w = c1 * c2 * c3 - s1 * s2 * s3;
	quat.x = s1 * s2 * c3 + c1 * c2 * s3;
	quat.y = s1 * c2 * c3 + c1 * s2 * s3;
	quat.z = c1 * s2 * c3 - s1 * c2 * s3;

}
